﻿using System;
using System.Collections.Generic;
using System.Text;
using GeometryCSharp;
using AGHFiraPlayer.PreviousDriver;
using AGHFiraPlayer.Drivers;

namespace AGHFiraPlayer.Strategies.StrategiesImpl
{
    class BasicGoalKeeperStrategy : GoalKeeperStrategy
    {
        private const double POSITION_ERROR = AGHDriver.ROBOT_DIAMETER;
        private const double MAXIMUM_DISTANCE_TO_KICK = AGHDriver.ROBOT_DIAMETER;
        private double HOME_Y_CORD = AGHDriver.home.y;

        private RobotController goalKeeper = null;

        private Point2D robotPosition, intersectionPoint, ballPos;
        private Boolean goalKeeperFarBeyondPenaltyArea, intersectionPointToFarAway;

        public BasicGoalKeeperStrategy(ref AGHDriver driver)
        {
            involvedRobots = new List<RobotController>(1);
            this.driver = driver;
        }

        override public int applicabilityLevel(ref List<IStrategy> choosenStrategies) 
        {
            if (involvedRobots.Count != 1)
            {
                involvedRobots.Clear();
                involvedRobots.Add(driver.GoalKeeper);
                goalKeeper = involvedRobots[0];
            }
            return MAX_APPLICABILITY + 1; 
        }

        override public void execute()
        {
            robotPosition = new Point2D(goalKeeper.robotState.x, goalKeeper.robotState.y);
            intersectionPoint = Predicator.findBestIntersectionPoint(robotPosition);  
            ballPos = new Point2D(driver.firaDriver.ballX, driver.firaDriver.ballY);

            goalKeeperFarBeyondPenaltyArea = goalKeeper.Position.x > AGHDriver.pitchArea.Width / 3;
            intersectionPointToFarAway = (double.IsNaN(intersectionPoint.x) || double.IsNaN(intersectionPoint.y)) ?
                                        true : intersectionPoint.GetDistance(goalKeeper.startupPosition) > AGHDriver.pitchArea.Width / 3;

            if (goalKeeperFarBeyondPenaltyArea || intersectionPointToFarAway)
                backToGoalAndRotateToBall();
            else
            {
                if (robotPosition.GetDistance(intersectionPoint) > POSITION_ERROR)
                    GoalKeeperDriver.DriveVelocity(goalKeeper.robotState, intersectionPoint.x, intersectionPoint.y, PotencialDriver.maxVelocity);
                else
                    fixRobotBallAngleAndRotateKick();
            }
        }

        private void backToGoalAndRotateToBall()
        {
            if (Math.Abs(goalKeeper.Position.x - AGHDriver.home.x) < AGHDriver.ROBOT_DIAMETER)
                HOME_Y_CORD = calculateHomeY();

            if (backToGoal(HOME_Y_CORD))
                SimpleMovingFunctions.rotateToBall(goalKeeper, driver.firaDriver);
        }

        private Boolean backToGoal(double yCoord)
        {
            Point2D target = new Point2D(AGHDriver.home.x, yCoord);
            if (target.GetDistance(robotPosition) > AGHDriver.ROBOT_DIAMETER)
                GoalKeeperDriver.DriveVelocity(goalKeeper.robotState, AGHDriver.home.x, yCoord, PotencialDriver.maxVelocity);
            else
            {
                goalKeeper.Stop();
                return true;
            }
            return false;
        }

        private double calculateHomeY()
        {
            if (ballPos.x > AGHDriver.pitchArea.Width / 2)
                return AGHDriver.pitchArea.Height / 2;

            double calculatedY = ballPos.y;
            if (ballPos.y > AGHDriver.goalEndY)
                calculatedY = AGHDriver.goalEndY;
            else if (ballPos.y < AGHDriver.goalStartY)
                calculatedY = AGHDriver.goalStartY;

            return calculatedY;
        }

        private void fixRobotBallAngleAndRotateKick()
        {
            double pitchHalfHeight = AGHDriver.pitchArea.Height / 2; ;
            Boolean isInLeftWingSector = ballPos.y < pitchHalfHeight ? true : false;

            SimpleMovingFunctions.rotateToBall(goalKeeper, driver.firaDriver);
            if (robotPosition.GetDistance(ballPos) < MAXIMUM_DISTANCE_TO_KICK)
                SimpleMovingFunctions.rotateKick(goalKeeper, isInLeftWingSector);
        }
    }
}
